#include "keeloq.h"
#include<stdio.h>


unsigned char NLF[2][2][2][2][2];

void KEELOQ_init(void)
{
    NLF[0][0][0][0][0]=0;
    NLF[0][0][0][0][1]=1;
    NLF[0][0][0][1][0]=1;
    NLF[0][0][0][1][1]=1;
    NLF[0][0][1][0][0]=0;
    NLF[0][0][1][0][1]=1;
    NLF[0][0][1][1][0]=0;
    NLF[0][0][1][1][1]=0;

    NLF[0][1][0][0][0]=0;
    NLF[0][1][0][0][1]=0;
    NLF[0][1][0][1][0]=1;
    NLF[0][1][0][1][1]=0;
    NLF[0][1][1][0][0]=1;
    NLF[0][1][1][0][1]=1;
    NLF[0][1][1][1][0]=1;
    NLF[0][1][1][1][1]=0;

    NLF[1][0][0][0][0]=0;
    NLF[1][0][0][0][1]=0;
    NLF[1][0][0][1][0]=1;
    NLF[1][0][0][1][1]=1;
    NLF[1][0][1][0][0]=1;
    NLF[1][0][1][0][1]=0;
    NLF[1][0][1][1][0]=1;
    NLF[1][0][1][1][1]=0;

    NLF[1][1][0][0][0]=0;
    NLF[1][1][0][0][1]=1;
    NLF[1][1][0][1][0]=0;
    NLF[1][1][0][1][1]=1;
    NLF[1][1][1][0][0]=1;
    NLF[1][1][1][0][1]=1;
    NLF[1][1][1][1][0]=0;
    NLF[1][1][1][1][1]=0;
}

//获取source第n个位数
unsigned char getBit(unsigned char source[],int n)
{
    unsigned char temp0=(unsigned char)1<<(n%8);
    unsigned char temp1=source[n/8]&temp0;
    if(temp1!=0)
    {
        return 1;
    }
    return 0;
}

//source带进位右移
unsigned char * RRC(unsigned char source[],char c,char n)
{
    int i=0;
    unsigned char temp;
    for(i=n-1;i>=0;i--)
    {
        temp=source[i];
        if(c!=0){
            source[i]=(source[i]>>1)|0x80;
        }else{
            source[i]=(source[i]>>1)&0x7f;
        }

        if((temp&0x01) != 0){
            c=1;
        }else{
            c=0;
        }
    }
    return source;
}

//source带进位左移
unsigned char * RLC(unsigned char source[],char c,char n)
{
    int i=0;
    unsigned char temp;
    for(i=0;i<n;i++)
    {
        temp=source[i];
        if(c!=0){
            source[i]=(source[i]<<1)|0x01;
        }else{
            source[i]=(source[i]<<1)&0xfe;
        }

        if((temp&0x80)!=0){
            c=1;
        }else{
            c=0;
        }
    }
    return source;
}

//加密
unsigned char * KEELOQ_Crypt(unsigned char *source,unsigned char key[])
{
    int i=0;
    unsigned char c=0;
    unsigned char nlf,y16,y0,k,result;
    KEELOQ_init();
    for (i = 0; i < 528; i++)
    {
        nlf=NLF[getBit(source, 31)][getBit(source, 26)][getBit(source, 20)][getBit(source, 9)][getBit(source, 1)];
        y16=getBit(source, 16);
        y0=getBit(source, 0);
        k=getBit(key, i%64);
        result=nlf^y16^y0^k;
        if (result!=0)
        {
            c=1;
        }
        else
        {
            c=0;
        }
        source=RRC(source,c,4);
    }
    return source;
}

//解密
unsigned char *  KEELOQ_Decrypt(unsigned char *source,unsigned char key[])
{
    int i=0;
    unsigned char c=0;
    unsigned char nlf,y15,y31,k,result;
    KEELOQ_init();
    for (i = 528; i >0; i--)
    {
        nlf=NLF[getBit(source, 30)][getBit(source, 25)][getBit(source, 19)][getBit(source, 8)][getBit(source, 0)];
        y15=getBit(source, 15);
        y31=getBit(source, 31);
        k=getBit(key, (i-1)%64);
        result=nlf^y15^y31^k;
        if ( result != 0)
            c=1;
        else
            c=0;

        source=RLC(source,c,4);
    }
    return source;
}





